#include "../../include/math/Matrix4.hpp"
using namespace klas;
using namespace math;

/*
 * File:   Matrix4.cpp
 * Author: Sean Chapel
 *
 * Created on June 8, 2009, 9:00 PM
 * Copyright 2009 Seoushi Games. All rights reserved.
 */

#include <stdio.h>
#include <string.h>
#include <cmath>

Matrix4::Matrix4()
{
    identity();
}


Matrix4::Matrix4(double m00, double m01, double m02, double m03,
                double m10, double m11, double m12, double m13,
                double m20, double m21, double m22, double m23,
                double m30, double m31, double m32, double m33)
{
    this->m00 = m00;
    this->m01 = m01;
    this->m02 = m02;
    this->m03 = m03;


    this->m10 = m10;
    this->m11 = m11;
    this->m12 = m12;
    this->m13 = m13;

    this->m20 = m20;
    this->m21 = m21;
    this->m22 = m22;
    this->m23 = m23;

    this->m30 = m30;
    this->m31 = m31;
    this->m32 = m32;
    this->m33 = m33;
}


Matrix4::Matrix4(const Matrix4 &m)
{
    *this = m;
}


Matrix4::~Matrix4()
{
}


void Matrix4::identity()
{
    memset(data, 0, 16 * sizeof(double) );
    m00 = m11 = m22 = m33 = 1.0f;
}


void Matrix4::transpose()
{
    Matrix4 m( m00, m10, m20, m30,
              m01, m11, m21, m31,
              m02, m12, m22, m32,
              m03, m13, m23, m33 );

    memcpy(data, m.data, 16 * sizeof(double) );
}


Matrix4 Matrix4::transposed() const
{
    return Matrix4( m00, m10, m20, m30,
                    m01, m11, m21, m31,
                    m02, m12, m22, m32,
                    m03, m13, m23, m33 );
}


Matrix4 Matrix4::rotateX( const double degrees )
{
    Matrix4 m;

    double  sr = (double)sin( (degrees * PI_OVER_180) );
    double  cr = (double)cos( (degrees * PI_OVER_180) );

    m.m11 =  cr;
    m.m21 = -sr;
    m.m12 =  sr;
    m.m22 =  cr;

    return m;
}


Matrix4 Matrix4::rotateY( const double degrees )
{
    Matrix4 m;

    double  sr = (double)sin( (degrees * PI_OVER_180) );
    double  cr = (double)cos( (degrees * PI_OVER_180) );

    m.m00 =  cr;
    m.m20 =  sr;
    m.m02 = -sr;
    m.m22 =  cr;

    return m;
}


Matrix4 Matrix4::rotateZ( const double degrees )
{
    Matrix4 m;

    double  sr = (double)sin( (degrees * PI_OVER_180) );
    double  cr = (double)cos( (degrees * PI_OVER_180) );

    m.m00 =  cr;
    m.m10 = -sr;
    m.m01 =  sr;
    m.m11 =  cr;

    return m;
}


Matrix4 Matrix4::scale( const double x, const double y, const double z )
{
    Matrix4 m;

    m.m00 = x;
    m.m11 = y;
    m.m22 = z;

    return m;
}


Matrix4 Matrix4::scale(const Vector2d &v)
{
  Matrix4 m;

  m.m00 = v.x;
  m.m11 = v.y;
  //m.m22 = v.z;

  return m;
}

Vector2d Matrix4::getScale() const
{
  Vector2d v;
  v.x = m00;
  v.y = m11;
  //v.z = m22;
  return v;
}


Matrix4 Matrix4::translate(double x, double y)
{
    Matrix4 m;

    m.m30 = x;
    m.m31 = y;
    //m.m32 = z;

    return m;
}


Matrix4 Matrix4::translate( const Vector2d &v )
{
  Matrix4 m;

  m.m30 = v.x;
  m.m31 = v.y;
  //m.m32 = v.z;

  return m;
}


Vector2d Matrix4::getTranslation() const
{
  Vector2d v;
  v.x = m13;
  v.y = m23;
  //v.z = m33;
  return v;
}

void Matrix4::subMatrix3x3(int i,int j,double* mat) const
{
  int ti, tj, idst=0, jdst=0;

  for ( ti = 0; ti < 4; ti++ )
  {
    if ( ti < i )
      idst = ti;
    
    else if ( ti > i )
      idst = ti-1;

    for ( tj = 0; tj < 4; tj++ )
    {
      if ( tj < j )
        jdst = tj;

      else if ( tj > j )
        jdst = tj-1;

      if ( ti != i && tj != j )
        mat[idst*3 + jdst] = data[ti*4 + tj];
    }
  }
}


void Matrix4::fastInverse(Matrix4* result)
{
    double *ptr = data;

    result->data[0]   = *(  ptr);
    result->data[4]   = *(++ptr);
    result->data[8]   = *(++ptr);

    double Tx             = *(++ptr);

    result->data[1]   = *(++ptr);
    result->data[5]   = *(++ptr);
    result->data[9]   = *(++ptr);

    double Ty             = *(++ptr);

    result->data[2]   = *(++ptr);
    result->data[6]   = *(++ptr);
    result->data[10]  = *(++ptr);

    double Tz         = *(++ptr);

    ptr = data;

    result->m03  = - *(  ptr) * Tx;
    result->m13  = - *(++ptr) * Tx;
    result->m23  = - *(++ptr) * Tx;
    ++ptr;
    result->m03  -=  *(++ptr) * Ty;
    result->m13  -=  *(++ptr) * Ty;
    result->m23  -=  *(++ptr) * Ty;
    ++ptr;
    result->m03  -=  *(++ptr) * Tz;
    result->m13  -=  *(++ptr) * Tz;
    result->m23  -=  *(++ptr) * Tz;
}


bool Matrix4::inverse(Matrix4* result) const
{
    double determinant = this->determinant();
    double matrixSub3x3[9];

    int i, j, sign;


    if ( fabs(determinant) < 0.0005 )
    {
        return false;
    }

    for ( i = 0; i < 4; i++ )
    {
        for ( j = 0; j < 4; j++ )
        {
            sign = 1 - ( (i +j) % 2 ) * 2;

            subMatrix3x3( i, j, matrixSub3x3 );

            double det =   matrixSub3x3[0] * ( matrixSub3x3[4]*matrixSub3x3[8] - matrixSub3x3[7]*matrixSub3x3[5] )
                            - matrixSub3x3[1] * ( matrixSub3x3[3]*matrixSub3x3[8] - matrixSub3x3[6]*matrixSub3x3[5] )
                            + matrixSub3x3[2] * ( matrixSub3x3[3]*matrixSub3x3[7] - matrixSub3x3[6]*matrixSub3x3[4] );

            result->data[i+j*4] = ( det * sign ) / determinant;
        }
    }

    return true;
}


bool Matrix4::inverse()
{
    Matrix4 inv;
    bool result = inverse(&inv);

    *this = inv;
    return result;
}


double Matrix4::determinant() const
{
    double det, result = 0, i = 1;
    double matrixSub3x3[9];

    for( int n = 0; n < 4; n++, i *= -1 )
    {
        subMatrix3x3( 0, n, matrixSub3x3 );

        det =     matrixSub3x3[0] * ( matrixSub3x3[4]*matrixSub3x3[8] - matrixSub3x3[7]*matrixSub3x3[5] )
                - matrixSub3x3[1] * ( matrixSub3x3[3]*matrixSub3x3[8] - matrixSub3x3[6]*matrixSub3x3[5] )
                + matrixSub3x3[2] * ( matrixSub3x3[3]*matrixSub3x3[7] - matrixSub3x3[6]*matrixSub3x3[4] );

        result += data[n] * det * i;
    }

    return result;
}


const bool Matrix4::operator==(const Matrix4 &m) const
{
    // guess all Matrix4 elements are the same
    return  (m00 == m.m00) &&
            (m01 == m.m01) &&
            (m02 == m.m02) &&
            (m03 == m.m03) &&

            (m10 == m.m10) &&
            (m11 == m.m11) &&
            (m12 == m.m12) &&
            (m13 == m.m13) &&

            (m20 == m.m20) &&
            (m21 == m.m21) &&
            (m22 == m.m22) &&
            (m23 == m.m23) &&

            (m30 == m.m30) &&
            (m31 == m.m31) &&
            (m32 == m.m32) &&
            (m33 == m.m33);
}

const bool Matrix4::operator!=( const Matrix4 &m)
{
    return !(m == *this);
}


const Matrix4& Matrix4::operator=(const Matrix4 &rhs)
{
    m00  = rhs.m00;
    m01  = rhs.m01;
    m02  = rhs.m02;
    m03  = rhs.m03;

    m10  = rhs.m10;
    m11  = rhs.m11;
    m12  = rhs.m12;
    m13  = rhs.m13;

    m20  = rhs.m20;
    m21  = rhs.m21;
    m22  = rhs.m22;
    m23  = rhs.m23;

    m30  = rhs.m30;
    m31  = rhs.m31;
    m32  = rhs.m32;
    m33  = rhs.m33;

    return *this;
}


const Matrix4& Matrix4::operator*=( const double &f)
{
    for(int i = 0; i < 16; i++)
    {
        data[i] *= f;
    }

    return *this;
}


const Matrix4& Matrix4::operator /= ( const double &f)
{
    for(int i = 0; i < 16; i++)
    {
        data[i] /= f;
    }

    return *this;
}


const Matrix4& Matrix4::operator *= (const Matrix4 &m)
{
    Matrix4 temp(*this);

    //  row 0
    m00  =  temp.m00 * m.m00;
    m01  =  temp.m01 * m.m00;
    m02  =  temp.m02 * m.m00;
    m03  =  temp.m03 * m.m00;

    m00 +=  temp.m10 * m.m01;
    m01 +=  temp.m11 * m.m01;
    m02 +=  temp.m12 * m.m01;
    m03 +=  temp.m13 * m.m01;

    m00 +=  temp.m20 * m.m02;
    m01 +=  temp.m21 * m.m02;
    m02 +=  temp.m22 * m.m02;
    m03 +=  temp.m23 * m.m02;

    m00 +=  temp.m30 * m.m03;
    m01 +=  temp.m31 * m.m03;
    m02 +=  temp.m32 * m.m03;
    m03 +=  temp.m33 * m.m03;


    //  row 1
    m10  =  temp.m00 * m.m10;
    m11  =  temp.m01 * m.m10;
    m12  =  temp.m02 * m.m10;
    m13  =  temp.m03 * m.m10;

    m10 +=  temp.m10 * m.m11;
    m11 +=  temp.m11 * m.m11;
    m12 +=  temp.m12 * m.m11;
    m13 +=  temp.m13 * m.m11;

    m10 +=  temp.m20 * m.m12;
    m11 +=  temp.m21 * m.m12;
    m12 +=  temp.m22 * m.m12;
    m13 +=  temp.m23 * m.m12;

    m10 +=  temp.m30 * m.m13;
    m11 +=  temp.m31 * m.m13;
    m12 +=  temp.m32 * m.m13;
    m13 +=  temp.m33 * m.m13;


    //  row 2
    m20  =  temp.m00 * m.m20;
    m21  =  temp.m01 * m.m20;
    m22  =  temp.m02 * m.m20;
    m23  =  temp.m03 * m.m20;

    m20 +=  temp.m10 * m.m21;
    m21 +=  temp.m11 * m.m21;
    m22 +=  temp.m12 * m.m21;
    m23 +=  temp.m13 * m.m21;

    m20 +=  temp.m20 * m.m22;
    m21 +=  temp.m21 * m.m22;
    m22 +=  temp.m22 * m.m22;
    m23 +=  temp.m23 * m.m22;

    m20 +=  temp.m30 * m.m23;
    m21 +=  temp.m31 * m.m23;
    m22 +=  temp.m32 * m.m23;
    m23 +=  temp.m33 * m.m23;


    //  row 3
    m30  =  temp.m00 * m.m30;
    m31  =  temp.m01 * m.m30;
    m32  =  temp.m02 * m.m30;
    m33  =  temp.m03 * m.m30;

    m30 +=  temp.m10 * m.m31;
    m31 +=  temp.m11 * m.m31;
    m32 +=  temp.m12 * m.m31;
    m33 +=  temp.m13 * m.m31;

    m30 +=  temp.m20 * m.m32;
    m31 +=  temp.m21 * m.m32;
    m32 +=  temp.m22 * m.m32;
    m33 +=  temp.m23 * m.m32;

    m30 +=  temp.m30 * m.m33;
    m31 +=  temp.m31 * m.m33;
    m32 +=  temp.m32 * m.m33;
    m33 +=  temp.m33 * m.m33;
    return *this;
}


Matrix4  Matrix4::operator * (const Matrix4 &m) const
{
    Matrix4 temp = *this;

    // row 0
    temp.m00  =  m00 * m.m00;
    temp.m01  =  m01 * m.m00;
    temp.m02  =  m02 * m.m00;
    temp.m03  =  m03 * m.m00;

    temp.m00 +=  m10 * m.m01;
    temp.m01 +=  m11 * m.m01;
    temp.m02 +=  m12 * m.m01;
    temp.m03 +=  m13 * m.m01;

    temp.m00 +=  m20 * m.m02;
    temp.m01 +=  m21 * m.m02;
    temp.m02 +=  m22 * m.m02;
    temp.m03 +=  m23 * m.m02;

    temp.m00 +=  m30 * m.m03;
    temp.m01 +=  m31 * m.m03;
    temp.m02 +=  m32 * m.m03;
    temp.m03 +=  m33 * m.m03;


    //  row 1
    temp.m10  =  m00 * m.m10;
    temp.m11  =  m01 * m.m10;
    temp.m12  =  m02 * m.m10;
    temp.m13  =  m03 * m.m10;

    temp.m10 +=  m10 * m.m11;
    temp.m11 +=  m11 * m.m11;
    temp.m12 +=  m12 * m.m11;
    temp.m13 +=  m13 * m.m11;

    temp.m10 +=  m20 * m.m12;
    temp.m11 +=  m21 * m.m12;
    temp.m12 +=  m22 * m.m12;
    temp.m13 +=  m23 * m.m12;

    temp.m10 +=  m30 * m.m13;
    temp.m11 +=  m31 * m.m13;
    temp.m12 +=  m32 * m.m13;
    temp.m13 +=  m33 * m.m13;


    //  row 2
    temp.m20  =  m00 * m.m20;
    temp.m21  =  m01 * m.m20;
    temp.m22  =  m02 * m.m20;
    temp.m23  =  m03 * m.m20;

    temp.m20 +=  m10 * m.m21;
    temp.m21 +=  m11 * m.m21;
    temp.m22 +=  m12 * m.m21;
    temp.m23 +=  m13 * m.m21;

    temp.m20 +=  m20 * m.m22;
    temp.m21 +=  m21 * m.m22;
    temp.m22 +=  m22 * m.m22;
    temp.m23 +=  m23 * m.m22;

    temp.m20 +=  m30 * m.m23;
    temp.m21 +=  m31 * m.m23;
    temp.m22 +=  m32 * m.m23;
    temp.m23 +=  m33 * m.m23;


    //  row 3
    temp.m30  =  m00 * m.m30;
    temp.m31  =  m01 * m.m30;
    temp.m32  =  m02 * m.m30;
    temp.m33  =  m03 * m.m30;

    temp.m30 +=  m10 * m.m31;
    temp.m31 +=  m11 * m.m31;
    temp.m32 +=  m12 * m.m31;
    temp.m33 +=  m13 * m.m31;

    temp.m30 +=  m20 * m.m32;
    temp.m31 +=  m21 * m.m32;
    temp.m32 +=  m22 * m.m32;
    temp.m33 +=  m23 * m.m32;

    temp.m30 +=  m30 * m.m33;
    temp.m31 +=  m31 * m.m33;
    temp.m32 +=  m32 * m.m33;
    temp.m33 +=  m33 * m.m33;
    return temp;
}

#include "../../include/GLLibs.hpp"
void Matrix4::apply()
{
    glLoadMatrixd(data);
}